#include "Moto.h"
#include "main.h"
#include "delay.h"
#include "tim.h"
#include "OLED.h"
#include "Pid.h"
#include "RotateCounter.h"

int speed_percentage = 0;

void Moto_State(MOTO_STATE_TYPE MOTO_STATE)
{
    HAL_GPIO_WritePin(Moto_State_GPIO_Port, Moto_State_Pin, MOTO_STATE);
}

void Toggle_Moto_State()
{
    if (HAL_GPIO_ReadPin(Moto_State_GPIO_Port, Moto_State_Pin) == ON)
    {
        HAL_GPIO_WritePin(Moto_State_GPIO_Port, Moto_State_Pin, GPIO_PIN_RESET);
        OLED_ShowString(8 * 8, 0, "OFF", 16, 0);
    }
    else if (HAL_GPIO_ReadPin(Moto_State_GPIO_Port, Moto_State_Pin) == OFF)
    {
        HAL_GPIO_WritePin(Moto_State_GPIO_Port, Moto_State_Pin, GPIO_PIN_SET);
        OLED_ShowString(8 * 8, 0, "ON ", 16, 1);
    }
}

void Moto_Direction(uint8_t MOTO_DIRECTION)
{
    if (MOTO_DIRECTION == CounterClockWise)
    {
        HAL_GPIO_WritePin(Moto_Clockwise_GPIO_Port, Moto_Clockwise_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(Moto_CounterClockwise_GPIO_Port, Moto_CounterClockwise_Pin, GPIO_PIN_SET);
    }
    else if (MOTO_DIRECTION == ClockWise)
    {
        HAL_GPIO_WritePin(Moto_Clockwise_GPIO_Port, Moto_Clockwise_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(Moto_CounterClockwise_GPIO_Port, Moto_CounterClockwise_Pin, GPIO_PIN_RESET);
    }
}

void Toggle_Moto_Direction()
{
    if (HAL_GPIO_ReadPin(Moto_CounterClockwise_GPIO_Port, Moto_CounterClockwise_Pin) == SET)
    {
        HAL_GPIO_WritePin(Moto_Clockwise_GPIO_Port, Moto_Clockwise_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(Moto_CounterClockwise_GPIO_Port, Moto_CounterClockwise_Pin, GPIO_PIN_RESET);
    }
    else if (HAL_GPIO_ReadPin(Moto_Clockwise_GPIO_Port, Moto_Clockwise_Pin) == SET)
    {
        HAL_GPIO_WritePin(Moto_Clockwise_GPIO_Port, Moto_Clockwise_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(Moto_CounterClockwise_GPIO_Port, Moto_CounterClockwise_Pin, GPIO_PIN_SET);
    }
}

void MotoControl_EXTI_Callback()
{
    // if (__HAL_GPIO_EXTI_GET_IT(Moto_State_Control_Pin) != 0x00u)
    if (HAL_GPIO_ReadPin(Moto_State_Control_GPIO_Port, Moto_State_Control_Pin) == GPIO_PIN_RESET)
    {
        Delay_ms(20);
        while (HAL_GPIO_ReadPin(Moto_State_Control_GPIO_Port, Moto_State_Control_Pin) == GPIO_PIN_RESET);
        Delay_ms(20);
        Toggle_Moto_State();
    }
    else if (__HAL_GPIO_EXTI_GET_IT(Moto_Speed_Control_Pin) != 0x00u)
    {
        Delay_ms(20);
        while (HAL_GPIO_ReadPin(Moto_Speed_Control_GPIO_Port, Moto_Speed_Control_Pin) == GPIO_PIN_RESET);
        Delay_ms(20);
        if (speed_percentage >= 0 && speed_percentage < 25)
        {
            speed_percentage = 25;
            Set_Moto_Speed(speed_percentage);
        }
        else if (speed_percentage >= 25 && speed_percentage < 50)
        {
            speed_percentage = 50;
            Set_Moto_Speed(speed_percentage);
        }
        else if (speed_percentage >= 50 && speed_percentage < 75)
        {
            speed_percentage = 75;
            Set_Moto_Speed(speed_percentage);
        }
        else if (speed_percentage >= 75 && speed_percentage < 100)
        {
            speed_percentage = 100;
            Set_Moto_Speed(speed_percentage);
        }
        else
        {
            speed_percentage = 0;
            Set_Moto_Speed(speed_percentage);
        }
        
    }
}

void Set_Moto_Speed(int percentage)
{
    uint16_t target_ccr = percentage * Max_Speed_CCR / 100.f;
    extern PidStruct pidstruct;
    pidstruct.target_ccr = target_ccr;
    // pidstruct.previous_ccr = Read_MotoSpeed();
    OLED_ShowNum(7 * 8, 2, percentage, 3, 16, 0);
}

void Switch_Moto_Speed(uint16_t ccr)
{
    __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, ccr);
}

int16_t Read_MotoSpeed()
{
    int16_t cnt = __HAL_TIM_GetCounter(&htim3);
    __HAL_TIM_SetCounter(&htim3, 0);
    float ccr = Max_Speed_CCR * cnt / 0.2 / SGM250_PARAMS / 22.5 / 4.f;
    OLED_Showdecimal(4 * 8, 6, cnt / 11.f / 4.f / 4.4 / 0.5, 2, 2, 16, 0);
    // OLED_Showdecimal(4 * 8, 6, ccr, 3, 2, 16, 0);
    return (int16_t)ccr;
}
